#include "MyRobot.h"

/**
 * This file includes the constructor for the RobotDemo class,
 * and also the Test() method used for testing code through
 * the driver station.
 */

//Anything on the robot physically that the program needs to
//communicate with MUST be initialized here before using!
//These MUST be in the same order as they are defined in MyRobot.h!
RobotDemo::RobotDemo() :
	// these must be initialized as they are declared in MyRobot.h
	driveFLeft(2, 2),
	driveBLeft(2, 1),
	driveFRight(2, 4),
	driveBRight(2, 3),
	myRobot(driveFLeft, driveBLeft, driveFRight, driveBRight),
	driveShiftLeft(9), //digital module must be in 2 or 6, where 2 is '1' and 6 is '2'
	driveShiftRight(10), //CRIO does NOT determine automatically, and explicitely telling it it is in '6' will not work
	loader(8),
	leftStick(1),
	rightStick(2),
	xbox(3),
	display(FOUR_ZONES),	//FOUR_ZONES, ONE_TWO_LINE_ZONE, TWO_ONE_LINE_ZONES
	pickUp(2, 5),
	indexer(2, 6),
	shooterFirst(2, 7),
	shooterSecond(2, 8),
	pyramidLifter(2, 9),
	pyramidIn(2, 1),
	pyramidOut(2, 2),
	indexerSensor(2, 5),
	encoderFirst(2, 3),
	encoderSecond(2, 4),
	RPMGearToothFirst(encoderFirst, false),
	RPMGearToothSecond(encoderSecond, false),
	PIDfirst(RPMP, RPMI, RPMD, &RPMGearToothFirst, &shooterFirst),
	PIDsecond(RPMP, RPMI, RPMD, &RPMGearToothSecond, &shooterSecond)
	
	//DEBUG PITFALLS:
	//commas between all initializations?
	//extra comma at the end?
{
	myRobot.SetExpiration(0.1);
	
	InitUDP();
	
	//Initialize the display zone headers
	dispBat.setHeader("Bat:");
	dispAimX.setHeader("AimY");
	dispAimY.setHeader("AimY");
	
	//Sets data in specific zones
	display.registerData(dispAimX, 1);
	display.registerData(dispAimY, 2);
	display.registerData(dispBat, 4);
	
	//Starts the display
	display.startDisplay();
	
	//Initialize PID controller
	PIDfirst.SetInputRange(0.0, 10000.0);
	PIDfirst.SetOutputRange(0.0, 1.0);
	PIDfirst.SetContinuous(false);
	PIDsecond.SetInputRange(0.0, 10000.0);
	PIDsecond.SetOutputRange(0.0, 1.0);
	PIDsecond.SetContinuous(false);
	
	//Begin threads for sending and receiving UDP bits
	displayShootFeed = true;
	pthread_t t1;
	pthread_t t2;
	
	pthread_create(&t1, NULL, &RobotDemo::SendUDP, this);
	pthread_create(&t2, NULL, &RobotDemo::RcvUDP, this);
}
	
/**
 * Runs during test mode
 */
void RobotDemo::Test()
{
	UpdateSmartDashboard(true); //init the LiveWindow smart dashboard
	
	LiveWindow::GetInstance()->SetEnabled(true);
	
	while (IsTest() && IsEnabled())
	{
		//Insert test code here
		LiveWindow::GetInstance()->Run();
	}
	
	LiveWindow::GetInstance()->SetEnabled(false);
}

START_ROBOT_CLASS(RobotDemo);


